#include "motorRunTest.h"
#include "mowerMotorInterface.h"
#include "ctrlMapMower.h"
#include "amLog.h"
#include "key.h"
#include "comm_data_pack.h"
#include "globalVariable.h"
#include "powerProc.h"
#include "module_config.h"
#include "emergencyProc.h"

#ifdef KEY_CONTROL_MOVEMENT 
#define KEY_RELEASE           ((uint8_t)0x00)
#define LEFT_SHIFT_KEY_PRESS  ((uint8_t)0x01)
#define OK_KEY_PRESS          ((uint8_t)0x02)
#define RIGHT_SHIFT_KEY_PRESS ((uint8_t)0x04)
#define BACK_KEY_PRESS        ((uint8_t)0x08)

int16_t debug_flag =0;
int16_t debug_speed =0;

/**
  * @brief  OK key controls start and stop; 
  *         left shift reduce speed by 0.1m/s each time
  *         right shift 
  * @param  void
  * @retval 0 : success
  * @retval -1 : error
  */
void motorTestPro(int16_t driverMotorSpeed,int16_t lawnMotorSpeed,int16_t trimmerSpeed)
{
    // static int16_t vxRef =0;
    // static int16_t wxRef =0;

    // static  uint16_t sendDelayCnt = 0; 
    // static  uint8_t okDoubleCnt = 0;
    // static uint16_t driverLeftMotorCnt = 0;
    // static uint16_t driverRightMotorCnt = 0;
    // static uint16_t lawnMotorCnt = 0;    
    // static uint16_t lawnMotorSpdCnt = 0;    
    // static uint16_t tcuMotorCnt = 0;  

    // static uint8_t s_OkKeyvalue = 0;
    // static uint8_t s_lastOkKeyvalue = 0; 
    // static uint8_t s_leftKeyvalue = 0;
    // static uint8_t s_lastleftKeyvalue = 0; 
    // static int16_t s_leftKeyCnt = 7; //Reduce speed by 0.1m/s for every 1 decrease

// #ifdef QUALITY_HARDWARE_TEST 
//         lawnMotorTest();
// #else    
//     if(debug_flag)    
//     {
//         setLawnMotorSpeed(debug_speed);
//         return;
//     }
//     s_OkKeyvalue = g_keyValue&OK_KEY_PRESS;
//     if(s_OkKeyvalue&&s_OkKeyvalue!=s_lastOkKeyvalue)
//     {
//         okDoubleCnt++;
//     }
//     s_lastOkKeyvalue = s_OkKeyvalue;

//     s_leftKeyvalue = g_keyValue&LEFT_SHIFT_KEY_PRESS;
//     if(s_leftKeyvalue&&s_leftKeyvalue!=s_lastleftKeyvalue)
//     {
//         s_leftKeyCnt--;
//         if(s_leftKeyCnt<0)
//         {
//             s_leftKeyCnt = 7;
//         }
//     }
//     s_lastleftKeyvalue = s_leftKeyvalue;

//     if(okDoubleCnt%2==1)
//     {
//         vxRef = driverMotorSpeed*s_leftKeyCnt/7;
//         wxRef = 0;
//         setLawnMotorSpeed(lawnMotorSpeed); 
        
//         tcuMotorCnt ++;
//         if(tcuMotorCnt >= 500)
//         {
//             tcuMotorCnt = 500;
//             TcuCtrlInfo_st info = {trimmerSpeed,1,1,0};
//             tcu_other_error_check_set(0);
//             tcu_ctrl_update(&info);
//         }
//         #if defined (TEST_DISPAY_HALL_BOARD) 
//             g_lift_motor_test = 1;
//             g_lift_motor_test_forward = 0;
//         #endif
//     }
//     else if(okDoubleCnt%2==0)
//     {
//         vxRef = 0;
//         wxRef = 0;
//         s_leftKeyCnt = 7;
//         setLawnMotorSpeed(0);
//         driverLeftMotorCnt = 0;
//         driverRightMotorCnt = 0;
//         lawnMotorCnt = 0;  
//         tcuMotorCnt = 0;
//         TcuCtrlInfo_st info = {0,1,1,0};
//         tcu_ctrl_update(&info);
        
//         #if defined (TEST_DISPAY_HALL_BOARD) 
//             g_lift_motor_test = 0; 
//         #endif
//     }
//     sendDelayCnt++;
//     if(sendDelayCnt%20 ==1)
//         mowerMotorSetSpeed(vxRef,wxRef); 
//     if(sendDelayCnt%200 ==1)
//         LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " %d  %d %d %d \n",vxRef,wxRef,okDoubleCnt,g_keyValue);
// #ifdef TEST_DISPAY_HALL_BOARD
// //#if 0
//         /* Determine whether the motor is right or not */
//         if(abs(vcuInterface.driverMotor.leftMotorIq)>500||abs(vcuInterface.driverMotor.leftMotorId)>500)
//         {
//             driverLeftMotorCnt++;
//             if(driverLeftMotorCnt>(2000/10))//2s
//             {
//                 driverLeftMotorCnt =(2000/10);
//                 buzzerPinSet();
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " ### driverLeftMotorCnt\n");
//             }        
//         }
//         else
//             driverLeftMotorCnt =0;

//         /* Determine whether the motor is right or not */
//         if(abs(vcuInterface.driverMotor.rightMotorIq)>500||abs(vcuInterface.driverMotor.rightMotorId)>500)
//         {
//             driverRightMotorCnt++;
//             if(driverRightMotorCnt>(2000/10))//2s
//             {
//                 driverRightMotorCnt =(2000/10);
//                 buzzerPinSet();
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " ### driverRightMotorCnt\n");
//             }        
//         }
//         else
//             driverRightMotorCnt =0;
        
//         static uint16_t driverMotorCnt = 0;
//         static int16_t leftMSpd = 0;
//         static int16_t rightMSpd = 0;
//         static int16_t liftMotorCnt = 0;
        
//         leftMSpd = (int16_t)(vcuInterface.driverMotor.leftMotorSpd/39*100);
//         rightMSpd = (int16_t)(vcuInterface.driverMotor.rightMotorSpd/39*100);
        
//         if((vxRef ==driverMotorSpeed)&&((abs(leftMSpd) - driverMotorSpeed> 60)||abs(rightMSpd) - driverMotorSpeed> 60) && abs(rightMSpd) > 50 && abs(leftMSpd) >50)
//         {
//             driverMotorCnt++;
//             if(driverMotorCnt>(3000/10))//3s
//             {
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " #### %d  %d %d  %d  %d \n",vcuInterface.driverMotor.leftMotorSpd,vcuInterface.driverMotor.rightMotorSpd,leftMSpd,rightMSpd,driverMotorSpeed);
//                 driverMotorCnt =(3000/10);
//                 buzzerPinSet();
//             }        
//         }
//         else
//             driverMotorCnt =0;
//         /* lawnmotor detect */
//         motorX.m_PID_Stru_Speed.hKp_Gain = 120;
//         motorX.m_PID_Stru_Speed.hKi_Gain = 20;        
//         //if((LawnMotor_Data.speed_set == lawnMotorSpeed&&LawnMotor_Data.speed<2000)||(abs(LawnMotor_Data.curr_iq)>1000))
//         if(abs(LawnMotor_Data.curr_iq)>1000)
//         {
//             lawnMotorCnt++;
//             if(lawnMotorCnt>(3000/10))//5s
//             {
//                 lawnMotorCnt =(3000/10);
//                 buzzerPinSet();
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " ### lawnMotorCnt\n");
// //                amAlarmSetMode(&gAlarm, AM_ALARM_MODE_STARTUP);
//             }         
//         }
//         else
//             lawnMotorCnt =0;
        
//         if((LawnMotor_Data.speed < (lawnMotorSpeed *9/10) && LawnMotor_Data.speed > 100) ||  LawnMotor_Data.speed > (lawnMotorSpeed *11/10) )
//         {
//             lawnMotorSpdCnt ++;
//             if(lawnMotorSpdCnt>(3000/10))//3s
//             {
//                 lawnMotorSpdCnt =(3000/10);
//                 amAlarmSetMode(&gAlarm, AM_ALARM_MODE_STARTUP);
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " ### lawnMotorSpdCnt\n");
//             }
//         }
//         else
//             lawnMotorSpdCnt =0;
            

//         if(1 == g_lift_motor_test)
//         {
//             liftMotorCnt ++;
//             if(liftMotorCnt>(3000/10))//3s
//             {
//                 liftMotorCnt =(3000/10);
//                 if(lift_motor.motor_current > 100 || (lift_motor.motor_current < 50 && lift_motor.motor_current > 30))
//                     amAlarmSetMode(&gAlarm, AM_ALARM_MODE_STARTUP);
// //                LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", " ### lift_motor.motor_current %d\n",lift_motor.motor_current);
//             }
//         }
//         else
//             liftMotorCnt =0;

// //#endif
// #endif

// #endif
}

uint32_t dbg_turn_cnt=0;
// 整车电机测试
void motorKeyTest(motorKeyTestDataTypedef *test_data, uint32_t periodMs)
{
    static uint8_t isEnterTest = 0;
    static uint32_t driverMotorCnt = 0;
    static uint32_t sendDelayCnt = 0;

    static uint8_t s_OkKeyvalue = 0;
    static uint8_t s_lastOkKeyvalue = 0; 
    static uint8_t okDoubleCnt = 0; 

    int16_t frontWheelSpeed = 0;
    int16_t rearWheelSpeed = 0;
    int16_t turnAngle = 0;
    int16_t lawnSpeed = 0;

    if(g_emergency_info.state || !osKernelRunning())
    {
        okDoubleCnt = 0; 
        isEnterTest = 0;
    }

    s_OkKeyvalue = astKey[KEY_OK].u8StableState;
    if((s_OkKeyvalue != s_lastOkKeyvalue) && (s_OkKeyvalue == KEY_STA_RELEASED))
    {
        okDoubleCnt++;
    }
    s_lastOkKeyvalue = s_OkKeyvalue;

    if(okDoubleCnt%2 == 1)
    {
        isEnterTest = 1;
    }
    else if(okDoubleCnt%2 == 0)
    {
        isEnterTest = 0;
    }

    if(isEnterTest /* && g_power_down_flag == 0 */)
    {
        driverMotorCnt++;
#if defined(TEST_KEY_CONTROL_MOTOR_VEHICLE_TEMP)
        frontWheelSpeed = 385;
        lawnSpeed = 2500;
        if(driverMotorCnt < (75000 / periodMs))
        {
            rearWheelSpeed = 1136;
            turnAngle = 0;
        }
        else if(driverMotorCnt < (150000 / periodMs))
        {
            rearWheelSpeed = 0;
            turnAngle = 0;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_EMC_RECE)
        frontWheelSpeed = 385;
        lawnSpeed = 2500;
        rearWheelSpeed = 1136;
        // turnAngle = 900;
        if(driverMotorCnt < (1000 / periodMs))
        {
            turnAngle = 900;
        }
        else if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = -900;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_EMC_MOTOR)
        frontWheelSpeed = 385;
        lawnSpeed = 2500;
        if(driverMotorCnt < (75000 / periodMs))
        {
            rearWheelSpeed = 1136;
            turnAngle = 0;
        }
        else if(driverMotorCnt < (150000 / periodMs))
        {
            rearWheelSpeed = 0;
            turnAngle = 0;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_TRYE)
        frontWheelSpeed = 385;
        lawnSpeed = 0;

        rearWheelSpeed = 1136;
        turnAngle = 1;
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_1)
        frontWheelSpeed = 385;
        lawnSpeed = 2500;

        rearWheelSpeed = 1136;
        turnAngle = 1;
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_2)
        frontWheelSpeed = 385;
        lawnSpeed = 2500;
        rearWheelSpeed = 1136;

        if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = 600;
        }
        else if(driverMotorCnt < (4000 / periodMs))
        {
            turnAngle = -600;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_B1)
        frontWheelSpeed = 0;
        lawnSpeed = 2500;
        rearWheelSpeed = 0;
        turnAngle = 1;
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_B2)
        frontWheelSpeed = 0;
        lawnSpeed = 0;
        rearWheelSpeed = 0;
        if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = 600;
        }
        else if(driverMotorCnt < (4000 / periodMs))
        {
            turnAngle = -600;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_B3)
        frontWheelSpeed = 0;
        lawnSpeed = 0;
        rearWheelSpeed = 1136;
        if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = 600;
        }
        else if(driverMotorCnt < (4000 / periodMs))
        {
            turnAngle = -600;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_NOISE_B4)
        frontWheelSpeed = 385;
        lawnSpeed = 0;
        rearWheelSpeed = 1136;
        if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = 600;
        }
        else if(driverMotorCnt < (4000 / periodMs))
        {
            turnAngle = -600;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_TURN_DURABILITY)
        frontWheelSpeed = 385;
        lawnSpeed = 0;
        rearWheelSpeed = 1136;
        if(driverMotorCnt < (2000 / periodMs))
        {
            turnAngle = 900;
        }
        else if(driverMotorCnt < (4000 / periodMs))
        {
            turnAngle = -900;
        }
        else
        {
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_TURN_DURABILITY2)
        frontWheelSpeed = 0;
        lawnSpeed = 0;
        rearWheelSpeed = 1136;
        if(driverMotorCnt < (1200 / periodMs))
        {
            turnAngle = 900;
        }
        else if(driverMotorCnt < (2400 / periodMs))
        {
            turnAngle = 1;
        }
        else if(driverMotorCnt < (3600 / periodMs))
        {
            turnAngle = -900;
        }
        else if(driverMotorCnt < (4800 / periodMs))
        {
            turnAngle = 1;
        }
        else
        {
            dbg_turn_cnt++;
            driverMotorCnt = 0;
        }
#elif defined(TEST_KEY_CONTROL_MOTOR_VEHICLE_TEMP2)
        static uint32_t turnMotorCnt = 0;
        frontWheelSpeed = 385;
        lawnSpeed = 2500;

        if(driverMotorCnt < (75000 / periodMs))
        {
            rearWheelSpeed = 1136;
        }
        else if(driverMotorCnt < (150000 / periodMs))
        {
            rearWheelSpeed = 0;
        }
        else
        {
            driverMotorCnt = 0;
        }

        turnMotorCnt++;
        if(turnMotorCnt < (5*60*60*1000 / periodMs))
        {
            turnAngle = 900;
        }
        else if(turnMotorCnt < (10*60*60*1000 / periodMs))
        {
            turnAngle = -900;
        }
        else
        {
            turnMotorCnt = 0;
        }
#endif
    }
    else
    {
        driverMotorCnt = 0;
        frontWheelSpeed = 0;
        lawnSpeed = 0;
        rearWheelSpeed = 0;
        turnAngle = 0;
    }

    test_data->frontWheelSpeed = frontWheelSpeed;
    test_data->lawnSpeed = lawnSpeed;
    test_data->turnAngle = turnAngle;
    test_data->rearWheelSpeed = rearWheelSpeed;

    sendDelayCnt++;
    if((sendDelayCnt % (200 / periodMs)) == 0)
    {
        LOG_TS(AM_LOG_LEVEL_INFO, "SPEED test", "EnterTest: %d frontWheelSpeed:%d rearWheelSpeed:%d turnAngle:%d lawnSpeed:%d turn count:%d\n", \
                                                    isEnterTest, frontWheelSpeed, rearWheelSpeed, turnAngle, lawnSpeed, dbg_turn_cnt);
    }
}

#endif


